/*
 * Command.cpp
 *
 *  Created on: Jun 15, 2016
 *      Author: Yang
 */
#include "Command.hpp"

#include "Console/Console.hpp"
#include "AHRS/AHRS.hpp"
#include "Altitude/Altitude.hpp"
#include "Hal/HAL.hpp"
#include "Compass/Compass.hpp"
extern "C"
{
#include "cmsis_os.h"
}

float Cmd_Task_Fre = 0.0001;
extern TIM_HandleTypeDef htim3;
/* Command list */
char const *diahacva = "display ahrs accel value";
uint8_t const diahacva_l = 24;

char const *diahgyva = "display ahrs gyro value";
uint8_t const diahgyva_l = 23;

char const *diahmava = "display ahrs mag value";
uint8_t const diahmava_l = 22;

char const *dimpva = "display mpu6000 value";
uint8_t const dimpva_l = 21;

char const *dimsva = "display ms5611 value";
uint8_t const dimsva_l = 20;

char const *dil3va = "display l3gd20 value";
uint8_t const dil3va_l = 20;

char const *getati = "get task time";
uint8_t const getati_l = 13;

char const *lita = "list task";
uint8_t const lita_l = 9;

char const *reboot = "reboot";
uint8_t const reboot_l = 6;
/* End of command list */

uint8_t cpr_str(Rx_Data* a, const char* b, uint8_t len)
{
    uint8_t i = 0;
    if (len != a->Len)
        return 0;
    for (; i < len; i++)
    {
        if (a->Data[i] != (uint8_t) b[i])
            return 0;
    }
    return 1;
}

void Error_command(void)
{
    console.printf((char*) "\"");
    console.Transmit(Console_RX.Data, Console_RX.Len, 100);
    console.printf((char*) "\"");
    console.printf((char*) "is not a Yunlong command,please check you command\r\n\r\n");
}

void Command_Task_Init(float fre)
{
    Cmd_Task_Fre = fre;
    console.printf((char*) "Yunlong>");
}

void Command_Task_Loop(void *pvParameters)
{
    char ac[TASK_NNUMBER * 40] = { 0 }, mu[TASK_NNUMBER * 40] = { 0 };
    portTickType xLastWakeTime;
    xLastWakeTime = xTaskGetTickCount();
    for (;;)
    {
        if (Console_RX.State == ready)
        {
            if (Console_RX.Len == 0)
            {
                console.printf((char*) "Yunlong>");
                Console_RxState_Clear
                ;
            }
            else
            {
                switch (Console_RX.Data[0])
                {
                case 'd':
                    if (cpr_str(&Console_RX, diahacva, diahacva_l))
                    {
                        console.printf((char*) "\r\n");
                        uint8_t i = 0;
                        for (; i < INS_MAX_INSTANCES; i++)
                        {
                            console.printf((char*) "accel[%d].x = %f\r\n", i, dcm._ins._accel[i].x);
                            console.printf((char*) "accel[%d].y = %f\r\n", i, dcm._ins._accel[i].y);
                            console.printf((char*) "accel[%d].z = %f\r\n", i, dcm._ins._accel[i].z);
                        }
                        console.printf((char*) "\r\n");
                        break;
                    }
                    else if (cpr_str(&Console_RX, diahgyva, diahgyva_l))
                    {
                        console.printf((char*) "\r\n");
                        uint8_t i = 0;
                        for (; i < INS_MAX_INSTANCES; i++)
                        {
                            console.printf((char*) "gyro[%d].x = %f\r\n", i, dcm._ins._gyro[i].x);
                            console.printf((char*) "gyro[%d].y = %f\r\n", i, dcm._ins._gyro[i].y);
                            console.printf((char*) "gyro[%d].z = %f\r\n", i, dcm._ins._gyro[i].z);
                        }
                        console.printf((char*) "\r\n");
                        break;
                    }
                    else if (cpr_str(&Console_RX, diahmava, diahmava_l))
                    {
                        console.printf((char*) "\r\n");
                        uint8_t i = 0;
                        for (; i < CPS_MAX_INSTANCES; i++)
                        {
                            console.printf((char*) "mag[%d].x = %f\r\n", i, _cps._mag[i].x);
                            console.printf((char*) "mag[%d].y = %f\r\n", i, _cps._mag[i].y);
                            console.printf((char*) "mag[%d].z = %f\r\n", i, _cps._mag[i].z);
                        }
                        console.printf((char*) "\r\n");
                        break;
                    }
                    else if (cpr_str(&Console_RX, dimpva, dimpva_l))
                    {
                        console.printf((char*) "\r\n");
                        console.printf((char*) "ax = %f, ay = %f, az = %f\r\n",
                                dcm._ins.MPU6000.accel.x, dcm._ins.MPU6000.accel.y,
                                dcm._ins.MPU6000.accel.z);
                        console.printf((char*) "\r\n");
                        console.printf((char*) "ax = %f, ay = %f, az = %f\r\n",
                                dcm._ins.MPU6000.accel.x, dcm._ins.MPU6000.accel.y,
                                dcm._ins.MPU6000.accel.z);
                        console.printf((char*) "gx = %f, gy = %f, gz = %f\r\n",
                                dcm._ins.MPU6000.gyro.x, dcm._ins.MPU6000.gyro.y,
                                dcm._ins.MPU6000.gyro.z);
                        console.printf((char*) "temperature = %f \r\n", dcm._ins.MPU6000.temp);
                        console.printf((char*) "\r\n");
                        break;
                    }
                    else if (cpr_str(&Console_RX, dimsva, dimsva_l))
                    {
                        console.printf((char*) "\r\n");
                        console.printf((char*) "pressure = %d\r\n", MS5611.pressure);
                        console.printf((char*) "\r\n");
                        break;
                    }
                    else if (cpr_str(&Console_RX, dil3va, dil3va_l))
                    {
                        console.printf((char*) "\r\n");
                        console.printf((char*) "gx = %f, gy = %f, gz = %f\r\n",
                                dcm._ins.L3GD20.gyro.x, dcm._ins.L3GD20.gyro.y,
                                dcm._ins.L3GD20.gyro.z);
                        console.printf((char*) "\r\n");
                        break;
                    }
                    Error_command();
                    break;

                case 'g':
                    if (cpr_str(&Console_RX, getati, getati_l))
                    {
                        vTaskGetRunTimeStats(mu);
                        console.printf((char*) "\r\n");
                        console.printf((char*) "Task            Abs Time        Time \r\n");
                        console.printf((char*) "*************************************\r\n");
                        console.Transmit((uint8_t*) mu, TASK_NNUMBER * 40, 100);
                        console.printf((char*) "\r\n");
                        break;
                    }
                    Error_command();
                    break;

                case 'l':
                    if (cpr_str(&Console_RX, lita, lita_l))
                    {
                        vTaskList(ac);
                        console.printf((char*) "\r\n");
                        console.printf((char*) "Name          State   Priority  Stack  Num \r\n");
                        console.printf((char*) "*******************************************\r\n");
                        console.Transmit((uint8_t*) ac, TASK_NNUMBER * 40, 100);
                        console.printf((char*) "\r\n");
                        break;
                    }

                    Error_command();
                    break;

                case 'r':
                    if (cpr_str(&Console_RX, reboot, reboot_l))
                    {
                        console.printf((char*) "\r\n");
                        console.printf((char*) "Do you sure want to reboot system?\r\n");
                        console.printf(
                                (char*) "Press \"Y\" to confirm your operation or any key to cancel:");
                        Console_RxState_Clear
                        ;
                        while (1)
                        {
                            if (Console_RX.State == ready)
                            {
                                if ((cpr_str(&Console_RX, (char*) "y", 1)
                                        || cpr_str(&Console_RX, (char*) "Y", 1)))
                                {
                                    console.printf((char*) "system rebooting...\r\n");
                                    hal.RTOSdelay_ms(1000);
                                    HAL_NVIC_SystemReset();
                                }
                                else
                                {
                                    console.printf((char*) "\r\nCancel\r\n");
                                    break;
                                }
                            }
                        }
                        console.printf((char*) "\r\n");
                        break;
                    }
                    Error_command();
                    break;
                default:
                    Error_command();
                    break;
                }

                console.printf((char*) "Yunlong>");
                Console_RxState_Clear
                ;
            }
        }
        vTaskDelayUntil(&xLastWakeTime, (1000 / Cmd_Task_Fre / portTICK_RATE_MS));
    }
}

